﻿
#include "sixjointrobot.h"
#include <math.h>
#include <stdlib.h>
#include <QDebug>

SixJointRobot::SixJointRobot(void)
{

}

SixJointRobot::~SixJointRobot(void)
{
}

int SixJointRobot::initialForIk(float robotDH[6][4])
{

//初始化ＤＨ
    for(int i=0;i<6;i++)
    {
//        for(int j=0;j<4;j++)
//        {
//            m_DH[i][j]=robotDH[i][j];
//        }
        m_DH[i][0]=robotDH[i][3];
        m_DH[i][1]=robotDH[i][1];
        m_DH[i][2]=robotDH[i][0];
        m_DH[i][3]=robotDH[i][2];

    }


    //计算机器人结构参数
    eox=m_DH[0][2];
    eoy=m_DH[0][3];
    l1=m_DH[1][2];
    l2=sqrt(m_DH[2][2]*m_DH[2][2]+m_DH[3][3]*m_DH[3][3]);
    bJ3=acos(m_DH[2][2]/l2);


    //计算连杆转移矩阵
    calculateLinkMatrix(m_DH[3][0], m_DH[3][1], m_DH[3][2], m_DH[3][3],  A4);
    calculateLinkMatrix(m_DH[4][0], m_DH[4][1], m_DH[4][2], m_DH[4][3],  A5);
    calculateLinkMatrix(m_DH[5][0], m_DH[5][1], m_DH[5][2], m_DH[5][3],  A6);
  //  calculateLinkMatrix(m_DH[6][0], m_DH[6][1], m_DH[6][2], m_DH[6][3],  A7);
   // calculateLiMatrix(A7, A7LI);//转移矩阵的逆矩阵计算

    float MatrixC[4][4];
    multiplymatrix( A4, A5, MatrixC);
    multiplymatrix( MatrixC, A6, A456);

}


int SixJointRobot::getIkConfig( float beforeJoint[6],  int fut[3])
{
    float poseMatrix[4][4];
    float jointSolution[6];
    calculatePosition(beforeJoint, poseMatrix);
    int ikSolution = getIkSolution(poseMatrix, beforeJoint, jointSolution);
    if (ikSolution < 0)
    {
        return ikSolution;
    }
    if (0 == m_bestSolutionIndex)
    {
        fut[0] = 1;
        fut[1] = 1;
        fut[2] = 1;
    }
    if (1 == m_bestSolutionIndex)
    {
        fut[0] = 0;
        fut[1] = 1;
        fut[2] = 1;
    }
    if (2 == m_bestSolutionIndex)
    {
        fut[0] = 1;
        fut[1] = 0;
        fut[2] = 1;
    }
    if (3 == m_bestSolutionIndex)
    {
        fut[0] = 0;
        fut[1] = 0;
        fut[2] = 1;
    }
    if (4 == m_bestSolutionIndex)
    {
        fut[0] = 1;
        fut[1] = 1;
        fut[2] = 0;
    }
    if (5 == m_bestSolutionIndex)
    {
        fut[0] = 0;
        fut[1] = 1;
        fut[2] = 0;
    }
    if (6 == m_bestSolutionIndex)
    {
        fut[0] = 1;
        fut[1] = 0;
        fut[2] = 0;
    }
    if (7 == m_bestSolutionIndex)
    {
        fut[0] = 0;
        fut[1] = 0;
        fut[2] = 0;
    }
    return 1;
}

int SixJointRobot::getIkSolutionFUT(float poseMatrix[4][4], int fut[6], float jointSolution[6])
{
    //初始化m_T60
        for(int i=0;i<4;i++)
        {
            for(int j=0;j<4;j++)
            {
                m_T60[i][j]=poseMatrix[i][j];
            }

        }

    for(int i=0;i<9;i++)//角度有效初始化
    {
    m_JIsValidate[i]=true;
    }

    //计算8组J1
    if(1!=calculateJ1( ))
        {
        return -1;
        }

    //计算8组J2J3
    if(1!=calculateJ2J3( ))
        {
        return -2;
        }

    if(m_JIsValidate[0]==true)//判断近端是否有解
    {
    calculateEulerTransferMatrix(0);//计算第i、i+1组解的欧拉转移矩阵T456

    //计算第i、i+1组解的J4J5J6
    if(1!=calculateEulerJ456(T456,  m_J,0))
        {
        return -3;
        }

    calculateEulerTransferMatrix(2);//计算第i、i+1组解的欧拉转移矩阵T456

    //计算第i、i+1组解的J4J5J6
    if(1!=calculateEulerJ456(T456,  m_J,2))
        {
        return -3;
        }

    }
    if(m_JIsValidate[4]==true)//判断远端是否有解
    {
    calculateEulerTransferMatrix(4);//计算第i、i+1组解的欧拉转移矩阵T456

    //计算第i、i+1组解的J4J5J6
    if(1!=calculateEulerJ456(T456,  m_J,4))
        {
        return -3;
        }


    calculateEulerTransferMatrix(6);//计算第i、i+1组解的欧拉转移矩阵T456

    //计算第i、i+1组解的J4J5J6
    if(1!=calculateEulerJ456(T456,  m_J,6))
        {
        return -3;
        }

    }

    if(1!=selectBestSolutionFUT(fut, jointSolution))
    {
        return -1;
    }

    return 1;
}

int SixJointRobot::getIkSolution(float poseMatrix[4][4], float beforeJoint[6], float jointSolution[6])//求解六轴串联机器人的位置逆解.
{

    //初始化m_T60
        for(int i=0;i<4;i++)
        {
            for(int j=0;j<4;j++)
            {
                m_T60[i][j]=poseMatrix[i][j];
            }

        }

	for(int i=0;i<9;i++)//角度有效初始化
	{
	m_JIsValidate[i]=true;
	}

    //计算8组J1
    if(1!=calculateJ1( ))
        {
        return -1;
        }

    //计算8组J2J3
    if(1!=calculateJ2J3( ))
        {
        return -2;
        }

	if(m_JIsValidate[0]==true)//判断近端是否有解
	{
	calculateEulerTransferMatrix(0);//计算第i、i+1组解的欧拉转移矩阵T456

    //计算第i、i+1组解的J4J5J6
    if(1!=calculateEulerJ456(T456,  m_J,0))
        {
        return -3;
        }

	calculateEulerTransferMatrix(2);//计算第i、i+1组解的欧拉转移矩阵T456

    //计算第i、i+1组解的J4J5J6
    if(1!=calculateEulerJ456(T456,  m_J,2))
        {
        return -3;
        }

	}
	if(m_JIsValidate[4]==true)//判断远端是否有解
	{
	calculateEulerTransferMatrix(4);//计算第i、i+1组解的欧拉转移矩阵T456

    //计算第i、i+1组解的J4J5J6
    if(1!=calculateEulerJ456(T456,  m_J,4))
        {
        return -3;
        }


	calculateEulerTransferMatrix(6);//计算第i、i+1组解的欧拉转移矩阵T456

    //计算第i、i+1组解的J4J5J6
    if(1!=calculateEulerJ456(T456,  m_J,6))
        {
        return -3;
        }

	}


    //比较最优解。满足条件１）没有奇异点。２）离初始点最近。
    if(1!=selectBestSolution(beforeJoint, jointSolution))
    {
        return -1;
    }

    return 1;


}

void SixJointRobot::multiplymatrix(float a[][4], float b[][4],float c[][4])//矩阵乘法函数
{
  c[0][0]=a[0][0]*b[0][0]+a[0][1]*b[1][0]+a[0][2]*b[2][0]+a[0][3]*b[3][0];
  c[1][0]=a[1][0]*b[0][0]+a[1][1]*b[1][0]+a[1][2]*b[2][0]+a[1][3]*b[3][0];
  c[2][0]=a[2][0]*b[0][0]+a[2][1]*b[1][0]+a[2][2]*b[2][0]+a[2][3]*b[3][0];
  c[3][0]=a[3][0]*b[0][0]+a[3][1]*b[1][0]+a[3][2]*b[2][0]+a[3][3]*b[3][0];

  c[0][1]=a[0][0]*b[0][1]+a[0][1]*b[1][1]+a[0][2]*b[2][1]+a[0][3]*b[3][1];
  c[1][1]=a[1][0]*b[0][1]+a[1][1]*b[1][1]+a[1][2]*b[2][1]+a[1][3]*b[3][1];
  c[2][1]=a[2][0]*b[0][1]+a[2][1]*b[1][1]+a[2][2]*b[2][1]+a[2][3]*b[3][1];
  c[3][1]=a[3][0]*b[0][1]+a[3][1]*b[1][1]+a[3][2]*b[2][1]+a[3][3]*b[3][1];

  c[0][2]=a[0][0]*b[0][2]+a[0][1]*b[1][2]+a[0][2]*b[2][2]+a[0][3]*b[3][2];
  c[1][2]=a[1][0]*b[0][2]+a[1][1]*b[1][2]+a[1][2]*b[2][2]+a[1][3]*b[3][2];
  c[2][2]=a[2][0]*b[0][2]+a[2][1]*b[1][2]+a[2][2]*b[2][2]+a[2][3]*b[3][2];
  c[3][2]=a[3][0]*b[0][2]+a[3][1]*b[1][2]+a[3][2]*b[2][2]+a[3][3]*b[3][2];

  c[0][3]=a[0][0]*b[0][3]+a[0][1]*b[1][3]+a[0][2]*b[2][3]+a[0][3]*b[3][3];
  c[1][3]=a[1][0]*b[0][3]+a[1][1]*b[1][3]+a[1][2]*b[2][3]+a[1][3]*b[3][3];
  c[2][3]=a[2][0]*b[0][3]+a[2][1]*b[1][3]+a[2][2]*b[2][3]+a[2][3]*b[3][3];
  c[3][3]=a[3][0]*b[0][3]+a[3][1]*b[1][3]+a[3][2]*b[2][3]+a[3][3]*b[3][3];
}

void SixJointRobot::calculateLinkMatrix(float oi, float afi, float ai, float di, float Ai[][4])//连杆转移矩阵计算
{

	Ai[0][0]=cos(oi);
	Ai[0][1]=-sin(oi)*cos(afi);
	Ai[0][2]=sin(oi)*sin(afi);
	Ai[0][3]=ai*cos(oi);

	Ai[1][0]=sin(oi);
	Ai[1][1]=cos(oi)*cos(afi);
	Ai[1][2]=-cos(oi)*sin(afi);
	Ai[1][3]=ai*sin(oi);

	Ai[2][0]=0;
	Ai[2][1]=sin(afi);
	Ai[2][2]=cos(afi);
	Ai[2][3]=di;

	Ai[3][0]=0;
	Ai[3][1]=0;
	Ai[3][2]=0;
	Ai[3][3]=1;

}

void SixJointRobot::calculateLiMatrix(float a[][4], float b[][4])//转移矩阵的逆矩阵计算
{
	b[0][0]=a[0][0];
	b[0][1]=a[1][0];
	b[0][2]=a[2][0];
	b[0][3]=-(a[0][0]*a[0][3]+a[1][0]*a[1][3]+a[2][0]*a[2][3]);

	b[1][0]=a[0][1];
	b[1][1]=a[1][1];
	b[1][2]=a[2][1];
	b[1][3]=-(a[0][1]*a[0][3]+a[1][1]*a[1][3]+a[2][1]*a[2][3]);

	b[2][0]=a[0][2];
	b[2][1]=a[1][2];
	b[2][2]=a[2][2];
	b[2][3]=-(a[0][2]*a[0][3]+a[1][2]*a[1][3]+a[2][2]*a[2][3]);

	b[3][0]=0;
	b[3][1]=0;
	b[3][2]=0;
	b[3][3]=1;
	
}

int SixJointRobot::calculateEulerJ456(float a[][4], float J[][6],int i)//用欧拉法,计算第i、i+1组解的J4J5J6
{


	J[i][4]=atan2(sqrt(a[2][0]*a[2][0]+a[2][1]*a[2][1]),a[2][2]);//返回[-pi, pi]角度.

    //当J5=0时。实际应用可以取0.01°的精度。用0°时的2种解
	if(J[i][4]>-0.01*M_PI/180 && J[i][4]<0.01*M_PI/180 )
	{
        //qDebug()<<"six robot ik solution failed: J5 singuraty!";
        return -3;

//		float sum=atan2(-a[0][1],a[0][0]);
//		//计算第i组解的J4J5J6***********************************
//		J[i][3]=m_bestDHJ[3];
//		J[i][4]=0;
//		J[i][5]=sum-m_bestDHJ[3];//用上次最佳解计算
//		//计算第i+1组解的J4J5J6***********************************
//		J[i+1][3]=sum-m_bestDHJ[5];
//		J[i+1][4]=0;
//		J[i+1][5]=m_bestDHJ[5];//用上次最佳解计算

	}
	//当J5=180°舍弃。
	else if(  J[i][4]>M_PI-0.01*M_PI/180 || J[i][4]<-M_PI+0.01*M_PI/180 )
	{
		//若为180°，则J4-J6=atan2(a[0][1],-a[0][0])
		m_JIsValidate[i]=false;
		m_JIsValidate[i+1]=false;

        //qDebug()<<"six robot ik solution failed: J5 singuraty!";
        return -3;

	}
	else//当J5不等于0或180°时
	{
		//计算第i组解的J4J5J6***********************************
		J[i][3]=atan2(a[1][2]/sin(J[i][4]),a[0][2]/sin(J[i][4]));//返回[-pi, pi]角度.
		J[i][4]=J[i][4];
		J[i][5]=atan2(a[2][1]/sin(J[i][4]),-a[2][0]/sin(J[i][4]));//返回[-pi, pi]角度.

		//计算第i+1组解的J4J5J6***********************************
		J[i+1][3]=J[i][3]+M_PI;
		J[i+1][4]=-J[i][4];
		J[i+1][5]=J[i][5]+M_PI;

	}
	

    return 1;

}


void SixJointRobot::calculatePosition(float DH[][4], float J[][6], int i, float P[][4])//位置姿态运动学正解
{

	//连杆转移矩阵
	float A1[4][4],A2[4][4],A3[4][4],A4[4][4],A5[4][4],A6[4][4],c[4][4],d[4][4];

	calculateLinkMatrix(J[i][0], DH[0][1], DH[0][2], DH[0][3],  A1);
	calculateLinkMatrix(J[i][1], DH[1][1], DH[1][2], DH[1][3],  A2);
	calculateLinkMatrix(J[i][2], DH[2][1], DH[2][2], DH[2][3],  A3);
	calculateLinkMatrix(J[i][3], DH[3][1], DH[3][2], DH[3][3],  A4);
	calculateLinkMatrix(J[i][4], DH[4][1], DH[4][2], DH[4][3],  A5);
	calculateLinkMatrix(J[i][5], DH[5][1], DH[5][2], DH[5][3],  A6);
    //calculateLinkMatrix(m_DH[6][0], m_DH[6][1], m_DH[6][2], m_DH[6][3],  A7);

	multiplymatrix( A1, A2, c);
	multiplymatrix( c, A3, d);
	multiplymatrix( d, A4, c);
	multiplymatrix( c, A5, d);
	multiplymatrix( d, A6, c);
	//multiplymatrix( d, A6, P);
    //multiplymatrix( c, A7, P);

}

int SixJointRobot::calculatePosition(float jointValue[6], float poseMatrix[][4])
{
		//连杆转移矩阵
	float A1[4][4],A2[4][4],A3[4][4],A4[4][4],A5[4][4],A6[4][4],c[4][4],d[4][4];

    calculateLinkMatrix(jointValue[0], m_DH[0][1], m_DH[0][2], m_DH[0][3],  A1);
    jointValue[1] += M_PI_2;
    calculateLinkMatrix(jointValue[1], m_DH[1][1], m_DH[1][2], m_DH[1][3],  A2);
    calculateLinkMatrix(jointValue[2], m_DH[2][1], m_DH[2][2], m_DH[2][3],  A3);
    calculateLinkMatrix(jointValue[3], m_DH[3][1], m_DH[3][2], m_DH[3][3],  A4);
    calculateLinkMatrix(jointValue[4], m_DH[4][1], m_DH[4][2], m_DH[4][3],  A5);
    calculateLinkMatrix(jointValue[5], m_DH[5][1], m_DH[5][2], m_DH[5][3],  A6);

	multiplymatrix( A1, A2, c);
	multiplymatrix( c, A3, d);
	multiplymatrix( d, A4, c);
	multiplymatrix( c, A5, d);
    multiplymatrix( d, A6, poseMatrix);

    return 1;
}


int SixJointRobot::calculateJ1(void)//计算8组J1
{

    //判断是否有顶置奇异点
    if( (m_T60[1][3]<0.02 && m_T60[1][3]>-0.02)
            && (m_T60[0][3]<0.02 && m_T60[0][3]>-0.02))
    {
        //qDebug()<<"six robot ik solution failed: top singuraty!";
        return -1;
    }
	m_J[0][0]=atan2(m_T60[1][3],m_T60[0][3]);//nearJ1  返回[-pi, pi]角度.
	m_J[1][0]=m_J[0][0];//nearJ1
	m_J[2][0]=m_J[0][0];//nearJ1
	m_J[3][0]=m_J[0][0];//nearJ1

	m_J[4][0]=m_J[0][0]+M_PI;//farJ1
	m_J[5][0]=m_J[4][0];//farJ1
	m_J[6][0]=m_J[4][0];//farJ1
	m_J[7][0]=m_J[4][0];//farJ1

    return 1;


}

int SixJointRobot::calculateJ2J3(void)//计算8组J2J3
{
	//当J1为近位置时*******
	float ox=sqrt(m_T60[1][3]*m_T60[1][3]+m_T60[0][3]*m_T60[0][3]);
	float oy=m_T60[2][3];
	float x1=ox-eox;
	float y1=oy-eoy;
	float b=atan2(y1,x1);//弦角度 返回[-pi, pi]角度.
	float angleRange=(x1*x1+y1*y1+l1*l1-l2*l2)/(2*l1*sqrt(x1*x1+y1*y1));
	float a;

    if(angleRange>=-0.92 && angleRange<=0.92)//检验目标点是否超出活动范围,离手臂成直线还差5度
	{
		a=acos(angleRange);//增减角，返回[0,180]范围内的角度
		m_J[0][1]=b-a;      //小J2，对应正的J3角度
		m_J[1][1]=m_J[0][1];//小J2，对应正的J3角度

		m_J[2][1]=b+a;      //大J2,对应负的J3角度
		m_J[3][1]=m_J[2][1];//大J2,对应负的J3角度

		m_J[0][2]=acos((x1*x1+y1*y1-l1*l1-l2*l2)/(2*l1*l2));//正J3
		m_J[1][2]=m_J[0][2];                                //正J3

		m_J[2][2]=-m_J[1][2];//负J3
		m_J[3][2]=m_J[2][2]; //负J3
	}
	else
	{
		m_JIsValidate[0]=false;
		m_JIsValidate[1]=false;
		m_JIsValidate[2]=false;
		m_JIsValidate[3]=false;

	}


    //当J1为远位置时********
	x1=ox+eox;
	b=atan2(y1,x1);
	angleRange=(x1*x1+y1*y1+l1*l1-l2*l2)/(2*l1*sqrt(x1*x1+y1*y1));

    if(angleRange>=-0.92 && angleRange<=0.92)//检验目标点是否超出活动范围
	{
		a=acos(angleRange);//增减角
		m_J[4][1]=M_PI-(b+a);//小J2，对应正的J3角度
		m_J[5][1]=m_J[4][1];//小J2，对应正的J3角度

		m_J[6][1]=M_PI-(b-a);//大J2,对应负的J3角度
		m_J[7][1]=m_J[6][1];//大J2,对应负的J3角度

		m_J[4][2]=acos((x1*x1+y1*y1-l1*l1-l2*l2)/(2*l1*l2));//正J3
		m_J[5][2]=m_J[4][2];                                //正J3

		m_J[6][2]=-m_J[5][2];//负J3
		m_J[7][2]=m_J[6][2]; //负J3

	}
	else
	{
		m_JIsValidate[4]=false;
		m_JIsValidate[5]=false;
		m_JIsValidate[6]=false;
		m_JIsValidate[7]=false;

	}

    // J3 singuraty!
    if(m_JIsValidate[0]==false && m_JIsValidate[1]==false && m_JIsValidate[2]==false &&m_JIsValidate[3]==false &&
            m_JIsValidate[4]==false && m_JIsValidate[5]==false && m_JIsValidate[6]==false &&m_JIsValidate[7]==false )
    {
        //qDebug()<<"six robot ik solution failed: J3 singuraty!";
        return -2;
    }


	m_J[0][2]=m_J[0][2]+bJ3;//更新J3为标准DH角度
	m_J[1][2]=m_J[1][2]+bJ3;//更新J3为标准DH角度
	m_J[2][2]=m_J[2][2]+bJ3;//更新J3为标准DH角度
	m_J[3][2]=m_J[3][2]+bJ3;//更新J3为标准DH角度
	m_J[4][2]=m_J[4][2]+bJ3;//更新J3为标准DH角度
	m_J[5][2]=m_J[5][2]+bJ3;//更新J3为标准DH角度
	m_J[6][2]=m_J[6][2]+bJ3;//更新J3为标准DH角度
	m_J[7][2]=m_J[7][2]+bJ3;//更新J3为标准DH角度

    return 1;

}

void SixJointRobot::calculateEulerTransferMatrix(int i)//第i组解的连杆转移矩阵
{
		
	calculateLinkMatrix(m_J[i][0], m_DH[0][1], m_DH[0][2], m_DH[0][3],  A1);
	calculateLinkMatrix(m_J[i][1], m_DH[1][1], m_DH[1][2], m_DH[1][3],  A2);
	calculateLinkMatrix(m_J[i][2], m_DH[2][1], m_DH[2][2], m_DH[2][3],  A3);

	float MatrixC[4][4];
	multiplymatrix( A1, A2, MatrixC);
	multiplymatrix( MatrixC, A3, A123);
	multiplymatrix( A123, A456, A1_6);//求得A1*A2*A3*A4*A5*A6

	calculateLiMatrix(A1_6, A1_6LI);//求A1_6的逆矩阵

    multiplymatrix( A1_6LI, m_T60, T456);//求得第2组解的连杆456的目标转移矩阵T456，位置元素误差百分比大

}

void SixJointRobot::MatrixRotationXself(float Matrixa[][4], float angle)
{
	float a[4][4]={{1,0,0,0}, {0,cos(angle),-sin(angle),0}, { 0,sin(angle),cos(angle),0}, { 0,0,0,1} };
    float b[4][4];	
	multiplymatrix(Matrixa,a,b);

	for(int i=0;i<4;i++)
   {
	   	for(int j=0;j<4;j++)
		{
			Matrixa[i][j]=b[i][j];
		  
		}
	  
	}

}
void SixJointRobot::MatrixRotationYself(float Matrixa[][4], float angle)
{
	float a[4][4]={{cos(angle),0,sin(angle),0}, {0,1,0,0}, { -sin(angle),0,cos(angle),0}, { 0,0,0,1} };
    float b[4][4];	
	multiplymatrix(Matrixa,a,b);

	for(int i=0;i<4;i++)
   {
	   	for(int j=0;j<4;j++)
		{
			Matrixa[i][j]=b[i][j];
		  
		}
	  
	}

}

void SixJointRobot::MatrixRotationZself(float Matrixa[][4], float angle)
{
	float a[4][4]={{cos(angle),-sin(angle),0,0}, {sin(angle),cos(angle),0,0}, { 0,0,1,0}, { 0,0,0,1} };
    float b[4][4];	
	multiplymatrix(Matrixa,a,b);

	for(int i=0;i<4;i++)
   {
	   	for(int j=0;j<4;j++)
		{
			Matrixa[i][j]=b[i][j];
		  
		}
	  
	}

}
void SixJointRobot::MatrixTranslateSelfcoordinate(float Matrixa[][4],float x,float y,float z)//参考TCP坐标平移
{
	float a[4][4]={{1,0,0,x}, {0,1,0,y}, { 0,0,1,z}, { 0,0,0,1} };
    float b[4][4];	
	multiplymatrix(Matrixa,a,b);

	for(int i=0;i<4;i++)
   {
	   	for(int j=0;j<4;j++)
		{
			Matrixa[i][j]=b[i][j];
		  
		}
	  
	}

}
void SixJointRobot::MatrixTranslateWorldcoordinate(float Matrixa[][4],float x,float y,float z)//参考世界坐标平移
{
	Matrixa[0][3]+=x;
	Matrixa[1][3]+=y;
	Matrixa[2][3]+=z;
}


void SixJointRobot::MatrixEqual(float a[][4], float b[][4])//b<=a
{
for(int i=0;i<4;i++)
   {
	   	for(int j=0;j<4;j++)
		{
			b[i][j]=a[i][j];
		  
		}
	  
	}
}

void SixJointRobot::MatrixEqual(float a[][4], float b[][4],int n)
{
for(int i=0;i<n;i++)
   {
	   	for(int j=0;j<4;j++)
		{
			b[i][j]=a[i][j];
		  
		}
	  
	}
}


void SixJointRobot::TransferDirectJToDHJ(float DJ[6], float DHJ[6],bool IsDirectAngle2DHJAngle)
{
	  if(IsDirectAngle2DHJAngle)
	  {
	  DHJ[0]=-DJ[0];
	  DHJ[1]=DJ[1]+M_PI_2;
	  DHJ[2]=-DJ[2];
	  DHJ[3]=DJ[3];
	  DHJ[4]=-DJ[4];
	  DHJ[5]=-DJ[5];
	  }
	  else
	  {
	  DJ[0]=-DHJ[0];
	  DJ[1]=DHJ[1]-M_PI_2;
	  DJ[2]=-DHJ[2];
	  DJ[3]=DHJ[3];
	  DJ[4]=-DHJ[4];
	  DJ[5]=-DHJ[5];

	  }


}
void SixJointRobot::TransferDirectJToDHJ(float DJ[6], float DHJ1,float DHJ2,float DHJ3,float DHJ4,float DHJ5,float DHJ6,bool IsDirectAngle2DHJAngle)
{
	  if(IsDirectAngle2DHJAngle)
	  {
	  DHJ1=-DJ[0];
	  DHJ2=DJ[1]+M_PI_2;
	  DHJ3=-DJ[2];
	  DHJ4=DJ[3];
	  DHJ5=-DJ[4];
	  DHJ6=-DJ[5];
	  }
	  else
	  {
	  DJ[0]=-DHJ1;
	  DJ[1]=DHJ2-M_PI_2;
	  DJ[2]=-DHJ3;
	  DJ[3]=DHJ4;
	  DJ[4]=-DHJ5;
	  DJ[5]=-DHJ6;

	  }


}

int SixJointRobot::selectBestSolutionFUT(int fut[6], float jointSolution[6])
{
    //判断是否存在位置不可达到的情况
    if(m_JIsValidate[0]==false && m_JIsValidate[1]==false
            &&m_JIsValidate[2]==false && m_JIsValidate[3]==false
            &&m_JIsValidate[4]==false && m_JIsValidate[5]==false
            &&m_JIsValidate[6]==false && m_JIsValidate[7]==false)
    {
        qDebug()<<"six joint robot get is solution failed: exceed arm length!";
        return -1; //位置不可达
    }

    //第二关节减去９０度,第二轴以垂直状态为０度
    for(int n=0;n<8;n++)
    {
        m_J[n][1] -= M_PI_2;
    }

    //所有解的角度表示转换，使在[-pi,pi]区间
    int n,m;
    for(n=0;n<8;n++)
    {
        for(m=0;m<6;m++)
        {
           if(m_J[n][m]>M_PI)
           {
               m_J[n][m]=m_J[n][m]-M_PI*2;
           }
           if(m_J[n][m]<-M_PI)
           {
               m_J[n][m]=m_J[n][m]+M_PI*2;
           }

        }
    }
    int bestIndex = 0;
    if(fut[0] == 1 && fut[1] == 1 && fut[2] == 1)
    {
        bestIndex = 0;
    }
    if(fut[0] == 0 && fut[1] == 1 && fut[2] == 1)
    {
        bestIndex = 1;
    }
    if(fut[0] == 1 && fut[1] == 0 && fut[2] == 1)
    {
        bestIndex = 2;
    }
    if(fut[0] == 0 && fut[1] == 0 && fut[2] == 1)
    {
        bestIndex = 3;
    }
    if(fut[0] == 1 && fut[1] == 1 && fut[2] == 0)
    {
        bestIndex = 4;
    }
    if(fut[0] == 0 && fut[1] == 1 && fut[2] == 0)
    {
        bestIndex = 5;
    }
    if(fut[0] == 1 && fut[1] == 0 && fut[2] == 0)
    {
        bestIndex = 6;
    }
    if(fut[0] == 0 && fut[1] == 0 && fut[2] == 0)
    {
        bestIndex = 7;
    }
    m_bestSolutionIndex=bestIndex;

    if (m_JIsValidate[bestIndex] == false)
    {
        return -1;
    }

    //最优解赋值
    for(int i=0;i<6;i++)
    {
        jointSolution[i]=m_J[bestIndex][i];
    }
    //多圈处理
    jointSolution[0] += fut[3] * M_PI * 2;
    jointSolution[3] += fut[4] * M_PI * 2;
    jointSolution[5] += fut[5] * M_PI * 2;
    return 1;
}

int SixJointRobot::selectBestSolution(float beforeJoint[6], float jointSolution[6])
{
    //判断是否存在位置不可达到的情况
    if(m_JIsValidate[0]==false && m_JIsValidate[1]==false
            &&m_JIsValidate[2]==false && m_JIsValidate[3]==false
            &&m_JIsValidate[4]==false && m_JIsValidate[5]==false
            &&m_JIsValidate[6]==false && m_JIsValidate[7]==false)
    {
        qDebug()<<"six joint robot get is solution failed: exceed arm length!";
        return -1; //位置不可达
    }

    //第二关节减去９０度,第二轴以垂直状态为０度
    for(int n=0;n<8;n++)
    {
        m_J[n][1] -= M_PI_2;
    }

	//所有解的角度表示转换，使在[-pi,pi]区间
    int n,m;
	for(n=0;n<8;n++)
	{
		for(m=0;m<6;m++)
		{
		   if(m_J[n][m]>M_PI)
		   {
			   m_J[n][m]=m_J[n][m]-M_PI*2;
		   }
		   if(m_J[n][m]<-M_PI)
		   {
			   m_J[n][m]=m_J[n][m]+M_PI*2;
		   }
		  
		}
			  
	}

     // 满足条件１）没有奇异点。２）离初始点最近。

        //计算各组解的J1+J2+J3+J5变化绝对值和
		float JChangedSum[8]={0,0,0,0,0,0,0,0};
		for( n=0;n<8;n++)
		{
            JChangedSum[n]=abs(m_J[n][0]-beforeJoint[0])+abs(m_J[n][1]-beforeJoint[1])+abs(m_J[n][2]-beforeJoint[2])
                           +abs(m_J[n][4]-beforeJoint[4]);
		}

        //探测有效解的第一组
        int bestIndex=0;
		for( n=0;n<8;n++)
		{
			if(m_JIsValidate[n]==true)
			{
				bestIndex=n;
				break;
				
			}
		}


        //循环比较角度变化最小值。
		for( n=0;n<8;n++)
		{
			if(JChangedSum[n]<JChangedSum[bestIndex] && m_JIsValidate[n]==true)
			{
				bestIndex=n;
			}
			
		}
        m_bestSolutionIndex=bestIndex;

        //最优解赋值
        for(int i=0;i<6;i++)
        {
            jointSolution[i]=m_J[bestIndex][i];
        }
        return 1;


}

float SixJointRobot::MaxCompare(float x, float y, float z)
{
		 float p,q;
		 
		 if(x<y) p=y;
		 else p=x;
		 
		 if(z<p) q = p;
		 else q = z;
		 return q;
}









